/*******************************************************************************
 * File Name: FlightController_Failsafe.h
 *
 * Version: 1.0
 *
 * Description:
 * 	
 * Author: ppks
 *
 * Notes:
 * 	
 *
 ********************************************************************************
 * Copyright (<year>)
 ********************************************************************************
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 ********************************************************************************/

#ifndef FLIGHTCONTROLLER_FAILSAFE_H_
#define FLIGHTCONTROLLER_FAILSAFE_H_

#include "config.h"
#include "FlightController.h"

/********************************************************************************/
/* Constants Definition                                                         */
/********************************************************************************/
#define RC_FAILSAFE_COUNTS_PER_100_MS   (100000 / RC_CHECK_TIME_PERIOD_IN_MICROS)

/********************************************************************************/
/* Macro Definition                                                             */
/********************************************************************************/

/********************************************************************************/
/* Exported Variables Declaration                                               */
/********************************************************************************/
extern uint16_t FlightController_RCFailsafeCounter;

/********************************************************************************/
/* Exported Functions Prototypes                                                */
/********************************************************************************/
extern void Failsafe_EnterRCFailsafeState(void);

inline void Failsafe_IncrementRCCounter(void)
{
    FlightController_RCFailsafeCounter++;
}

inline void Failsafe_ClearRCCounter(void)
{
    FlightController_RCFailsafeCounter = 0;
}

inline void Failsafe_DecreaseRCCounter(void)
{
    /* Decrease failsafe counter by half value of the activation period */
    FlightController_RCFailsafeCounter -= ((FAILSAFE_DELAY * RC_FAILSAFE_COUNTS_PER_100_MS) >> 1);
}

inline bool Failsafe_IsRCFailsafeActive(void)
{
    if (FlightController_RCFailsafeCounter > (FAILSAFE_DELAY * RC_FAILSAFE_COUNTS_PER_100_MS))
    {
        return true;
    }
    return false;
}

inline bool Failsafe_IsTimeToStopMotors(void)
{
    if (FlightController_RCFailsafeCounter > ((FAILSAFE_DELAY + FAILSAFE_OFF_DELAY) * RC_FAILSAFE_COUNTS_PER_100_MS))
    {
        return true;
    }
    return false;
}
#endif /* FLIGHTCONTROLLER_FAILSAFE_H_ */
